#include "MathUtils.h"

#include <cassert>
#include <cstdlib>

using namespace DirectX;

float 
randomFloat() {
	return static_cast<float> (rand()) / static_cast<float> (RAND_MAX);
}

float 
randomFloat(const float min, const float max) {
    return min + randomFloat() * (max - min);
}

void 
inverseTranspose(CXMMATRIX m, XMMATRIX& out) { 
    out = m; 
    out.r[3] = XMVectorSet(0.0f, 0.0f, 0.0f, 1.0f); 
    XMVECTOR determinant = XMMatrixDeterminant(m); 

    out = XMMatrixTranspose(XMMatrixInverse(&determinant, m)); 
}

float 
height(const float x, const float z) {
    return 0.3f * (z * sinf(0.1f * x) + x * cosf(0.1f * z));
}

float 
eulerAngle(const float x, const float y) {
    float theta = 0.0f;

    // Quadrant I or IV
    if (x >= 0.0f) {
        // If x = 0, then atanf(y/x) = +pi/2 if y > 0
        //                atanf(y/x) = -pi/2 if y < 0
        theta = atanf(y / x); // in [-pi/2, +pi/2]

        if (theta < 0.0f) {
            theta += 2.0f * XM_PI; // in [0, 2*pi).
        }            
    } else {
        // Quadrant II or III
        theta = atanf(y / x) + XM_PI; // in [0, 2*pi).
    }

    return theta;
}

bool 
areEquals(const float num1, const float num2, const float epsilon /*= 1.0E-5*/) 
{
    const float s = fabs(num1 - num2);

    return s < epsilon;
}

float 
rotationAngle(FXMVECTOR& src, FXMVECTOR& dest) {
    // Normalize vectors
    const XMVECTOR unitSrc = XMVector3Normalize(src);
    const XMVECTOR unitDest = XMVector3Normalize(dest);

    // Compute cross product vector
    XMVECTOR crossVec;
    crossVec = XMVector3Cross(unitSrc, unitDest);

    // Check if cross product is zero vector
	bool isZeroVec = isZero(crossVec);

    // Compute dot product
    const XMVECTOR dotVec = XMVector3Dot(unitSrc, unitDest);
    const float dotProduct = XMVectorGetX(dotVec);

    // src and dest are aligned
    if (isZeroVec && areEquals(dotProduct, 0.0f)) {
        return 0.0f;
    }

    // If crossVec is pointing out of the screen,
    // then we should rotate counterclockwise
    // from dest to src. Otherwise we
    // should rotate clockwise
    const float angle = acos(dotProduct);
    const float crossVecY = XMVectorGetY(crossVec);
    return (crossVecY > 0.0f) ? angle : -angle;        
}

void 
rotateAroundYAxis(XMVECTOR& v, const float angle) {
    const float cosAngle = cos(angle);
    const float sinAngle = sin(angle);

    // Compute dot product
    const XMVECTOR yAxis = XMVectorSet(0.0f, 1.0f, 0.0f, 1.0f);
    const XMVECTOR dot = XMVector3Dot(yAxis, v);
    const float dotProduct = XMVectorGetX(dot);

    // v1 = v * cos
    XMVECTOR v1 = v;
    XMVectorScale(v1, cosAngle);

    // v2 = (yAxis x v) * sin
    XMVECTOR v2 = XMVector3Cross(yAxis, v);
    v2 = XMVectorScale(v2, sinAngle);
    
    XMVECTOR v3 = XMVectorSet(0.0f, 1.0f, 0.0f, 1.0f);
    const float scale = dotProduct * (1.0f - cosAngle);
    v3 = XMVectorScale(v3, scale);
    
    v = XMVectorAdd(v1, v2);
    v = XMVectorAdd(v, v3);
}

void 
set(const float x, const float y, const float z, DirectX::XMFLOAT3A& vec) {
    XMVECTOR tmp = XMVectorSet(x, y, z, 1.0f);
    XMStoreFloat3A(&vec, tmp);
}

void 
set(const float x, const float y, const float z, DirectX::XMFLOAT3& vec) {
    XMVECTOR tmp = XMVectorSet(x, y, z, 1.0f);
    XMStoreFloat3(&vec, tmp);
}

void 
setLength(XMVECTOR& v, const float len) {
	assert(!areEquals(len, 0.0f));

    v = XMVector3Normalize(v);
    v = XMVectorScale(v, len);
}

bool 
isZero(DirectX::FXMVECTOR& vec) {
	const XMVECTOR zeroVec = XMVectorZero();
	uint32_t result;
	XMVectorEqualR(&result, vec, zeroVec);
	
	return XMComparisonAllTrue(result);	
}